#include <math.h>
#include "math3d.h"

const mat4 mat4::identity = mat4(1,0,0,0, 0,1,0,0, 0,0,1,0, 0,0,0,1);

mat4 mat4::makePerspectiveMat(float fov, float aspect, float near, float far)
{
  mat4 m;

  float xymax = near * tanf(fov/2.0f);
  float ymin = -xymax;
  float xmin = -xymax;

  float width = xymax - xmin;
  float height = xymax - ymin;

  float depth = far - near;
  float q = -(far + near) / depth;
  float qn = -2 * (far * near) / depth;

  float w = 2 * near / width;
  w = w / aspect;
  float h = 2 * near / height;

  m.m[0][0]  = w;
  m.m[0][1]  = 0;
  m.m[0][2]  = 0;
  m.m[0][3]  = 0;

  m.m[1][0]  = 0;
  m.m[1][1]  = h;
  m.m[1][2]  = 0;
  m.m[1][3]  = 0;

  m.m[2][0] = 0;
  m.m[2][1] = 0;
  m.m[2][2] = q;
  m.m[2][3] = -1;

  m.m[3][0] = 0;
  m.m[3][1] = 0;
  m.m[3][2] = qn;
  m.m[3][3] = 0;

  return m;
}

mat4 mat4::operator*(const mat4 &rhs) const
{
  mat4 r;
  for(int i=0 ; i<4 ; i++)
  {
    register const float rhs_i0 = rhs.m[i][0];
    register float ri0 = m[0][0] * rhs_i0;
    register float ri1 = m[0][1] * rhs_i0;
    register float ri2 = m[0][2] * rhs_i0;
    register float ri3 = m[0][3] * rhs_i0;
    for (int j=1 ; j<4 ; j++)
    {
      register const float rhs_ij = rhs.m[i][j];
      ri0 += m[j][0] * rhs_ij;
      ri1 += m[j][1] * rhs_ij;
      ri2 += m[j][2] * rhs_ij;
      ri3 += m[j][3] * rhs_ij;
    }
    r.m[i][0] = ri0;
    r.m[i][1] = ri1;
    r.m[i][2] = ri2;
    r.m[i][3] = ri3;
  }
  return r;
}

mat4 mat4::makeRotateMat(float x, float y, float z)
{
  mat4 r;
	float s1 = sinf(x); float c1 = cosf(x);
	float s2 = sinf(y); float c2 = cosf(y);
	float s3 = sinf(z); float c3 = cosf(z);
	return mat4(
	  c2*c3,           c2*s3,           -s2,
	  c1*-s3+s1*s2*c3, c1*c3+s1*s2*s3,  s1*c2,
    s1*s3+c1*s2*c3,  -s1*c3+c1*s2*s3, c1*c2);
}

vec3 mat4::operator*(const vec3 &rhs) const
{
  return vec3(
    m[0][0]*rhs.x + m[1][0]*rhs.y + m[2][0]*rhs.z + m[3][0],
    m[0][1]*rhs.x + m[1][1]*rhs.y + m[2][1]*rhs.z + m[3][1],
    m[0][2]*rhs.x + m[1][2]*rhs.y + m[2][2]*rhs.z + m[3][2]);
}

vec4 mat4::operator*(const vec4 &rhs) const
{
  return vec4(
    m[0][0]*rhs.x + m[1][0]*rhs.y + m[2][0]*rhs.z + m[3][0]*rhs.w,
    m[0][1]*rhs.x + m[1][1]*rhs.y + m[2][1]*rhs.z + m[3][1]*rhs.w,
    m[0][2]*rhs.x + m[1][2]*rhs.y + m[2][2]*rhs.z + m[3][2]*rhs.w,
    m[0][3]*rhs.x + m[1][3]*rhs.y + m[2][3]*rhs.z + m[3][3]*rhs.w);
}

mat4 mat4::inverted() const
{
  mat4 d;
  d.m[0][0] = m[1][1]*m[2][2] - m[1][2]*m[2][1];
  d.m[0][1] = m[2][1]*m[0][2] - m[2][2]*m[0][1];
  d.m[0][2] = m[0][1]*m[1][2] - m[0][2]*m[1][1];
  d.m[0][3] = 0.0f;
  d.m[1][0] = m[1][2]*m[2][0] - m[1][0]*m[2][2];
  d.m[1][1] = m[2][2]*m[0][0] - m[2][0]*m[0][2];
  d.m[1][2] = m[0][2]*m[1][0] - m[0][0]*m[1][2];
  d.m[1][3] = 0.0f;
  d.m[2][0] = m[1][0]*m[2][1] - m[1][1]*m[2][0];
  d.m[2][1] = m[2][0]*m[0][1] - m[2][1]*m[0][0];
  d.m[2][2] = m[0][0]*m[1][1] - m[0][1]*m[1][0];
  d.m[2][3] = 0.0f;
  d.m[3][0] = 0.0f;
  d.m[3][1] = 0.0f;
  d.m[3][2] = 0.0f;
  d.m[3][3] = 1.0f;

  float dd = m[0][0]*d.m[0][0] + m[1][0]*d.m[0][1] + m[2][0]*d.m[0][2];
  float oodd = 1.0f/dd; //d==0.0f ? FLT_MAX : 1.0f/d;

  d.m[0][0] *= oodd; d.m[0][1] *= oodd; d.m[0][2] *= oodd;
  d.m[1][0] *= oodd; d.m[1][1] *= oodd; d.m[1][2] *= oodd;
  d.m[2][0] *= oodd; d.m[2][1] *= oodd; d.m[2][2] *= oodd;

  d.m[3][0] = -m[3][0]*d.m[0][0] - m[3][1]*d.m[1][0] - m[3][2]*d.m[2][0];
  d.m[3][1] = -m[3][0]*d.m[0][1] - m[3][1]*d.m[1][1] - m[3][2]*d.m[2][1];
  d.m[3][2] = -m[3][0]*d.m[0][2] - m[3][1]*d.m[1][2] - m[3][2]*d.m[2][2];

  return d;
}
